﻿#include <tuple>
#include <QElapsedTimer>
#include "GSMotorController.h"
#include "bitutils.h"
#include "WaitUtils.h"

GSMotorController::GSMotorController(QObject *parent):BaseMotorController(parent)
{
    _velocityMap={
        {AxisType::AXIS_X,0x00},
        {AxisType::AXIS_Y,0x03},
        {AxisType::AXIS_Z,0x06}
    };
    _axisMotorMap={
        {AxisType::AXIS_X,0x01},
        {AxisType::AXIS_Y,0x02},
        {AxisType::AXIS_Z,0x04}
    };
}

void GSMotorController::SetCommunication(QList<BaseCommunication *> communication)
{
    BaseDevice::SetCommunication(communication);
    if(communication.empty())
    {
        throw QString("GSMotorController 通讯配置失败,无通讯实体!");
    }
    _sender=dynamic_cast<BaseSendReply*>(communication.first());
}

void GSMotorController::SetCurrentAxisType(BaseMotorController::AxisType axisType)
{
    BaseMotorController::SetCurrentAxisType(axisType);
}

int GSMotorController::SetVelocity(VEL_TYPE_ENUM eVelType, double dVelocity)
{
    int iErrorCode = 0;
    double dTemp = dVelocity;
    dTemp *=1000;
    char iAddr = _velocityMap[_axisType];
    //将相应的速度写到对应的寄存器
    iErrorCode = writeDataToRegister(iAddr,dTemp);//发送设定命令
    return iErrorCode;
}

int GSMotorController::GetVelocity(VEL_TYPE_ENUM eVelType, double &dVelocity)
{
    char iAddr = _velocityMap[_axisType];
    QByteArray rec;
    readDataFromRegister(iAddr,rec);
    QByteArray hexResult = rec.toHex().mid(4,8);
    dVelocity=("ffffffff" == hexResult)?0:hexResult.toInt(0,16);
    if(eVelType == VEL_TOPSPEED) {
        dVelocity /=1000;
    }
    return 0;
}

int GSMotorController::RelativeMove(double dRelativeMove)
{
    //设置运行参数、启动电机等待运行完成
    int iErrorCode = 0;
    QByteArray readByteArray;       //串口返回的数据

    iErrorCode = setMotorPara(MoveType::Relative,_axisType,dRelativeMove);
    if(0 != iErrorCode) {
        qDebug()<<__FUNCTION__<<__LINE__<<"setMotorPara error";
        return -1;
    }

    uchar cmdCode = 0x00;//启动
    iErrorCode = writeMotorCmd(cmdCode,readByteArray);

    if(0 != iErrorCode) {
        qDebug()<<__FUNCTION__<<__LINE__<<"Send start motor command error!";
    } else { //等待数据返回
        //qDebug()<<__FUNCTION__<<__LINE__<<readByteArray;
        if("FE0100FF" == readByteArray.toHex().toUpper()) {
            iErrorCode = 0;
        } else {
            iErrorCode = -1;
        }
    }
    WaitForMotorStop();
    return iErrorCode;
}

void GSMotorController::WaitForMotorStop()
{
    WaitUtils::WaitMsNoException(100);
    QElapsedTimer waitTime;
    waitTime.start();
    while (waitTime.elapsed() < 2*MAX_WAIT_TIME&&GetMotorStatus()) {
        WaitUtils::WaitMsNoException(200);
    }
}

int GSMotorController::AbsoluteMove(double dMove)
{
    int iErrorCode = 0;
    QByteArray readByteArray;       //串口返回的数据
    setMotorPara(MoveType::Absolute,_axisType,dMove);

    uchar cmdCode = 0x00;//启动
    iErrorCode = writeMotorCmd( cmdCode,readByteArray);
    if(0 != iErrorCode) {
        qDebug()<<__FUNCTION__<<__LINE__<<"Send start motor command error!";
    } else { //等待数据返回
        //qDebug()<<__FUNCTION__<<__LINE__<<readByteArray;
        if("FE0100FF" == readByteArray.toHex().toUpper()) {
            iErrorCode = 0;
        } else {
            iErrorCode = -1;
        }
    }
    //需停止一段时间再检测，否则得不到数据
    WaitForMotorStop();
    return iErrorCode;
}

int GSMotorController::SetCurrentPositionToZero()
{
    int iErrorCode = 0;
    QByteArray readByteArray;       //串口返回的数据

    uchar cmdCode = 0x02;        //设置原点
    iErrorCode = writeMotorCmd(cmdCode,readByteArray);
    if("FE0100FF" != readByteArray.toHex().toUpper())
    {
        iErrorCode = -1;
    }
    QMap<AxisType,uchar> map={
        {AXIS_X,0x09},
        {AXIS_Y,0x0A},
        {AXIS_Z,0x0B},
    };
    int dPosition=0;
    writeDataToRegister(map[_axisType],abs(dPosition*10));
    return iErrorCode;
}

int GSMotorController::GetPosition(double &dPosition)
{
    int iErrorCode = 0;
    QByteArray readByteArray;
    uchar cmdCode = 0x08;//读位置
    iErrorCode = writeMotorCmd(cmdCode,readByteArray);
    if(0 != iErrorCode) {
        qDebug()<<__FUNCTION__<<__LINE__<<"Send start motor command error!";
    } else { //等待数据返回
        if( 16 != readByteArray.toHex().count()) {
            iErrorCode = -1;
        } else {
            QByteArray hexResult = readByteArray.toHex().mid(4,8);
            uint temp = hexResult.toUpper().toUInt(0,16);
            dPosition = qint32(temp);
        }

        dPosition /= (1.0*2000);
    }

    return iErrorCode;
}


bool GSMotorController::GetMotorStatus()
{
//    uchar poreAddr=GetCurrentAxisAddr();
    //0x03表示读电机状态
    QByteArray readByteArray;
    if(writeMotorCmd(0x03,readByteArray))
    {
        QByteArray hexResult = readByteArray.toHex().mid(4,2);
        int value = hexResult.toInt(0,16);
        bool isRunning=(value!=0);
        return isRunning;
    }
    return false;
}

//限位功能还未想好，使用QMap保存限位状态？
bool GSMotorController::SetLimit(BaseMotorController::DirectionType limitType, bool enable)
{
    return true;
}

bool GSMotorController::GetLimit(BaseMotorController::DirectionType limitType, bool &enable)
{
    return true;
}

bool GSMotorController::Stop()
{
    QByteArray readByteArray;       //串口返回的数据
    uchar cmdCode = 0x01;        //急停
    writeMotorCmd(cmdCode,readByteArray);
    return "FE0100FF" == readByteArray.toHex().toUpper();
}

bool GSMotorController::writeDataToRegister(uchar addr, double value)
{
    int iValue = static_cast<int>(abs(value));
    QList<uchar> data=BitUtils::Uint32ConvertToUCharList(iValue);
    QByteArray sendByteArray=CreateCommand(0x08,addr,data);
    QByteArray readByteArray;
    _sender->SendReply(sendByteArray,readByteArray);
    return "FE0100FF" == readByteArray.toHex().toUpper();
}

bool GSMotorController::readDataFromRegister(uchar addr,QByteArray &readData)
{
    QByteArray sendByteArray=CreateCommand(0x07,addr);  //向控制轴发送的数据命令：
    return _sender->SendReply(sendByteArray,readData);
}

int GSMotorController::setMotorPara(MoveType bAbsolutionMove, AxisType eAxis, double dMoveValue)
{
    int iErrorCode = 0;
    QByteArray readByteArray;       //串口返回的数据

    bool bForwardFlag = (dMoveValue > 0);

    QList<uchar> sendValue;
    //端口
    QMap<AxisType, QList<int>> ucMap = {
        {AXIS_X,{0x01, 0x02, 0x01}},
        {AXIS_Y,{0x02, 0x10, 0x08}},
        {AXIS_Z,{0x04, 0x80, 0x40}}
    };
    uchar ucData = GetCurrentAxisAddr();
    //端口
    sendValue.append(0x00);     //电机地址高字节
    sendValue.append(ucData);   //电机地址低字节

    //方向
    if(bAbsolutionMove==MoveType::Absolute)
    {
        sendValue.append(0x02);
    } else
    {
        if(bForwardFlag)
        {   //正转
            sendValue.append(0x00);
        }
        else
        {   //反转
            sendValue.append(0x01);
        }
    }

    //速度
    double dVelocity = 0;
    GetVelocity(VEL_TOPSPEED, dVelocity);
    int runVel = static_cast<int>(dVelocity*1000);
    sendValue.append(BitUtils::ConvertToUint8List(runVel));

    //脉冲数
    dMoveValue *= 2000;
    int iValue = static_cast<int>(abs(dMoveValue));
    sendValue.append(BitUtils::Uint32ConvertToUint8List(iValue));

    //限位端口还是触发端口？,x0-x8，四字节,往什么方向跑就触发那个方向的触发端口
    uchar limitData = bForwardFlag?ucMap[eAxis].at(1):ucMap[eAxis].at(2);
    sendValue<<0x00<<0x00<<0x00<<limitData;

    //触发方式
    sendValue<<0x00;  //有信号急停
    //单位
    sendValue<<0x00;  //使用毫米单位

    int AcceVel = 10;
    sendValue<<(AcceVel & 0xFF);  //加速度

    int deAcceVel = 10;
    sendValue<<(deAcceVel & 0xFF);  //减速度
    QByteArray sendByteArray=CreateCommand(0x0b,sendValue);

    iErrorCode = _sender->SendReply(sendByteArray,readByteArray);//发送设定命令
     if("FE0100FF" != readByteArray.toHex().toUpper())
     {
        iErrorCode=-1;
     }

    return iErrorCode;
}

bool GSMotorController::writeMotorCmd(uchar cmd,QByteArray &readData)
{
    QList<uchar> listSendData;
    listSendData.append(0x00);     //电机端口号2个字节，高字节
    uchar portAddr=GetCurrentAxisAddr();
    listSendData.append(portAddr);  //电机端口号2个字节，低字节
    listSendData.append(cmd);  //命令码
    QByteArray sendByteArray=CreateCommand(0x0c,listSendData);
    return _sender->SendReply(sendByteArray,readData);
}


QByteArray GSMotorController::CreateCommand(uchar funcCode,uchar addr, QList<uchar> value)
{
    QList<uchar> values;
    values<<0x00<<addr;
    values.append(value);
    return CreateCommand(funcCode,values);
}

//最基本的指令发送方法
QByteArray GSMotorController::CreateCommand(uchar funcCode, QList<uchar> value)
{
    //    格式 命令头 站号 功能码 地址 数据 校验
    //    发送 0xFE 0x01 0x08 字节2~字节1 字节4~字节1 16位
    //    格式 命令头 站号 校验
    //    返回 0xFE 0x01 16 位
    QList<uchar> ret;
    ret<<0xfe<<0x01<<funcCode;
    ret.append(value);
    QList<uchar> cumCrc=BitUtils::SumCheck(ret);
    ret.append(cumCrc);
    QByteArray retBr;
    for(uchar item:ret)
    {
        retBr.append(item);
    }
    return retBr;
}

uchar GSMotorController::GetCurrentVelAddr()
{
    return _velocityMap[_axisType];
}

uchar GSMotorController::GetCurrentAxisAddr()
{
    return _axisMotorMap[_axisType];
}
